#include "WProgram.h"
void nunchuck_init();
void send_zero();
void getn();
void print();
char nunchuk_decode_byte(char x);
#include "Wire.h"
#include "string.h"
#include "stdio.h"
int servoPin1 = 12;
int servoPin2 = 11;
float minPulse = 400;
float maxPulse = 2500;
float pulse1 = minPulse;
float lastPulse1 = pulse1;
float pulse2 = minPulse;
float lastPulse2 = pulse2;

// the time in milliseconds of last pulse
int lastPulseTime = 0;
int refreshTime = 20;

int analogValue = 0;
int analogPin = 2;

uint8_t outbuf[6];		// array to store arduino output
int cnt = 0;
int ledPin = 13;

void
setup ()
{
  beginSerial (19200);
  Serial.print ("Finished setup\n");
  pinMode (servoPin1, OUTPUT);
  pinMode (servoPin2, OUTPUT);


  Wire.begin ();		// join i2c bus with address 0x52
  nunchuck_init ();		// send the initilization handshake
}

void
nunchuck_init ()
{
  Wire.beginTransmission (0x52);	// transmit to device 0x52
  Wire.send (0x40);		// sends memory address
  Wire.send (0x00);		// sends sent a zero.  
  Wire.endTransmission ();	// stop transmitting
}

void
send_zero ()
{
  Wire.beginTransmission (0x52);	// transmit to device 0x52
  Wire.send (0x00);		// sends one byte
  Wire.endTransmission ();	// stop transmitting
}

void
loop ()
{
  // pulse the servo again if rhe refresh time (20 ms) have passed:
  if (millis () - lastPulseTime >= refreshTime)
    {
      getn ();
      digitalWrite (servoPin1, HIGH);
      delayMicroseconds (pulse1);
      digitalWrite (servoPin1, LOW);

      digitalWrite (servoPin2, HIGH);
      delayMicroseconds (pulse2);
      digitalWrite (servoPin2, LOW);
      // save the time of the last pulse
      lastPulseTime = millis ();
    }
  delay (10);

}

void
getn ()
{
  Wire.requestFrom (0x52, 6);	// request data from nunchuck
  while (Wire.available ())
    {
      outbuf[cnt] = nunchuk_decode_byte (Wire.receive ());	// receive byte as an integer
      digitalWrite (ledPin, HIGH);	// sets the LED on
      cnt++;
    }

  // If we recieved the 6 bytes, then go print them
  if (cnt >= 5)
    {
      print ();
    }

  cnt = 0;
  send_zero ();			// send the request for next bytes
}

// Print the input data we have recieved
// accel data is 10 bits long
// so we read 8 bits, then we have to add
// on the last 2 bits.  That is why I
// multiply them by 2 * 2
void
print ()
{
  int joy_x_axis = outbuf[0];
  int joy_y_axis = outbuf[1];
  int accel_x_axis = outbuf[2] ;
  int accel_y_axis = outbuf[3] ;
  int accel_z_axis = outbuf[4] ;

  int z_button = 0;
  int c_button = 0;

  // byte outbuf[5] contains bits for z and c buttons
  // it also contains the least significant bits for the accelerometer data
  // so we have to check each bit of byte outbuf[5]
  if ((outbuf[5] >> 0) & 1)
    {
      z_button = 1;
    }
  if ((outbuf[5] >> 1) & 1)
    {
      c_button = 1;
    }

  if ((outbuf[5] >> 2) & 1)
    {
      accel_x_axis += 2;
    }
  if ((outbuf[5] >> 3) & 1)
    {
      accel_x_axis += 1;
    }

  if ((outbuf[5] >> 4) & 1)
    {
      accel_y_axis += 2;
    }
  if ((outbuf[5] >> 5) & 1)
    {
      accel_y_axis += 1;
    }

  if ((outbuf[5] >> 6) & 1)
    {
      accel_z_axis += 2;
    }
  if ((outbuf[5] >> 7) & 1)
    {
      accel_z_axis += 1;
    }

  Serial.print (accel_x_axis, DEC);
  Serial.print ("\t");
  if (z_button)
    {
      float a = ((float) accel_x_axis - 75.0) / 175.0;
      float tmpPulse = minPulse + ((maxPulse - minPulse) * a);

      if ((tmpPulse / lastPulse1) > 1.02 || (tmpPulse / lastPulse1) < .98)
	{
	  pulse1 = tmpPulse;
	  lastPulse1 = pulse1;
	}

      a = ((float) accel_y_axis - 75.0) / 175.0;
      tmpPulse = minPulse + ((maxPulse - minPulse) * a);

      if ((tmpPulse / lastPulse2) > 1.02 || (tmpPulse / lastPulse2) < .98)
	{
	  pulse2 = tmpPulse;
	  lastPulse2 = pulse2;
	}

    }
  else
    {

      float a = ((float) joy_x_axis - 29.0) / 219.0;
      float tmpPulse = minPulse + ((maxPulse - minPulse) * a);

      if ((tmpPulse / lastPulse1) > 1.02 || (tmpPulse / lastPulse1) < .98)
	{
	  pulse1 = tmpPulse;
	  lastPulse1 = pulse1;
	}

      a = ((float) joy_y_axis - 29.0) / 219.0;
      tmpPulse = minPulse + ((maxPulse - minPulse) * a);

      if ((tmpPulse / lastPulse2) > 1.02 || (tmpPulse / lastPulse2) < .98)
	{
	  pulse2 = tmpPulse;
	  lastPulse2 = pulse2;
	}
    }
  Serial.print (pulse1, DEC);
  Serial.print ("\t");

  //Serial.print (accel_y_axis, DEC);
  //Serial.print ("\t");

  Serial.print (pulse2, DEC);
  Serial.print ("\t");
  Serial.print ("\r\n");
}

// Encode data to format that most wiimote drivers except
// only needed if you use one of the regular wiimote drivers
char
nunchuk_decode_byte (char x)
{
  x = (x ^ 0x17) + 0x17;
  return x;
}
